#include "Processor.h"

// CONSTS
#define M_PI       3.14159265358979323846f
#define M_PI_2     1.57079632679489661923f

#define max(a,b)            (((a) > (b)) ? (a) : (b))
#define min(a,b)            (((a) < (b)) ? (a) : (b))

CProcessor::CProcessor(void)
{
	Reset();
	m_Mode = COMBO;
}

CProcessor::~CProcessor(void)
{
}

void CProcessor::Reset(void)
{
	m_RotationMatrix.CreateIdentity();
}

void CProcessor::GetRollPitchYaw(float& roll, float& pitch, float& yaw)
{
	roll = m_Roll;
	pitch = m_Pitch;
	yaw = m_Yaw;
}

void CProcessor::GetRollPitchYawRates(float& rollRate, float& pitchRate, float& yawRate)
{
	rollRate = m_RollRate;
	pitchRate = m_PitchRate;
	yawRate = m_YawRate;
}

CMatrix CProcessor::GetRotationMatrix(void)
{
	return m_RotationMatrix;
}

float CProcessor::DegToRadian(float deg)
{
	return deg * (float)M_PI / 180.0f;
}

void CProcessor::AddNewMeasurement(SDataFile* df, float timeStep)
{
	// gyros [deg/s]
	m_RollRate = -df->Rots[0];
	m_PitchRate = -df->Rots[1];
	m_YawRate = df->Rots[2];

	CMatrix rotRoll, rotPitch, rotYaw, totalGyrosChange;
	rotRoll.CreateRotationX(DegToRadian(m_RollRate * timeStep));
	rotPitch.CreateRotationY(DegToRadian(m_PitchRate * timeStep));
	rotYaw.CreateRotationZ(DegToRadian(m_YawRate * timeStep));
	totalGyrosChange = rotRoll*rotPitch*rotYaw;
	
	// accelerometers [m/s^2]
	// 1. up vector (Z)
	CVector groundVector(df->Levels[1],-df->Levels[0], -df->Levels[2]);
	CVector up;
	up = -groundVector;
	up.Normalize();
	// 2. side vector (Y)
	CVector tempForward(1, 0, 0); // always look forward
	CVector side;
	side = up * tempForward; // cross product
	side.Normalize();
	// 3. forward vector (X)
	CVector forward;
	forward = side * up;
	forward.Normalize();

	CMatrix totalAccAbsolute;
	totalAccAbsolute.FromVectors(forward, side, up);
	
	// add yaw rotation
	float currentYaw = m_Yaw;
	CMatrix yawRotation;
	yawRotation.CreateRotationZ((float)M_PI_2-currentYaw);
	totalAccAbsolute = totalAccAbsolute * yawRotation;
	
	// filtered accelerometers
	CMatrix accFiltered;
	CQuaternion originalRotation, newAccRotation, finalRotation;
	originalRotation.FromMatrix(m_RotationMatrix);
	newAccRotation.FromMatrix(totalAccAbsolute);
	finalRotation.FromSlerp(originalRotation,newAccRotation,0.1f);
	finalRotation.Normalize();
	accFiltered.FromQuaternion(finalRotation);

	switch( m_Mode)
	{
		case GYROS:
			m_RotationMatrix = totalGyrosChange*m_RotationMatrix;
			break;
		case ACC:
			m_RotationMatrix = totalAccAbsolute;
			break;
		case ACCFILTERED:
			m_RotationMatrix = accFiltered;
			break;
		case COMBO:
			m_RotationMatrix = accFiltered;
			m_RotationMatrix = totalGyrosChange*m_RotationMatrix;
			break;
	}

	// calculate yaw/pitch/roll
	m_RotationMatrix.GetRollPitchYaw(m_Roll, m_Pitch, m_Yaw);
}
